#include "CameraBase.h"
#include "CameraRegister.h"

#ifdef _WIN32
#include <Windows.h>
#else 
#include <unistd.h>
#endif

#ifdef _WIN32
#define SLEEP(a) Sleep(a)
#else
#define SLEEP(a) usleep(a*1000)
#endif

namespace movesense {

uint8_t ChannelOffset(uint8_t channel)
{
	uint8_t i=0;

	if (channel == 0)
	{
		return 0;
	}

	while ( (channel & 0x01) != 0x01 )
	{
		i++;
		channel = channel >> 1;
	}

	return i;
}

CameraBase::CameraBase(CameraInfo info)
{
	this->info = info;
}


CameraBase::~CameraBase(void)
{
}

int CameraBase::WriteRegister(uint16_t addr, uint8_t value)
{
    std::unique_lock<std::mutex> lck (regMutex);
	int ret = SetSaturation(addr);
	if (ret != MS_SUCCESS)
	{
		return ret;
	}

	return SetWhiteBalance(value);
}

int CameraBase::ReadRegister(uint16_t addr, uint8_t *value)
{
    std::unique_lock<std::mutex> lck (regMutex);
	int ret = SetSaturation(addr);
	if (ret != MS_SUCCESS)
	{
		return ret;
	}

	long whiteBalance;
	ret = GetWhiteBalance(&whiteBalance);
	if (ret != MS_SUCCESS)
	{
		return ret;
	}

	*value = whiteBalance & 0xff;

	return MS_SUCCESS;
}

int CameraBase::SetChannelSwitch( uint16_t addr, bool onoff, uint8_t channels )
{
	uint8_t channelsNow;
	int res = GetRegister(addr, &channelsNow);
	if (res != MS_SUCCESS)
	{
		return res;
	}

	if (onoff)
	{
		channelsNow |= channels;
	}
	else
	{
		channelsNow &= ~channels;
	}

	return SetRegister(addr, channelsNow);
}

int CameraBase::SetChannelRegister( uint16_t addr, uint8_t value, uint8_t channel/*=MS_CHANNEL_FORWARD*/ )
{
	int channelOffset = ChannelOffset(channel);

	return SetRegister(addr+OFFSET_CHANNEL_CTRL_PARA*channelOffset, value);
}

int CameraBase::SetRegister( uint16_t addr, uint8_t value )
{
	int res = WriteRegister(addr, value);
	if (res != MS_SUCCESS)
	{
		return res;
	}

	uint8_t valueCheck;
	res = ReadRegister(addr, &valueCheck);
	if (res != MS_SUCCESS)
	{
		return res;
	}

	if (value != valueCheck)
	{
		return MS_FAIL;
	}

	return MS_SUCCESS;
}

int CameraBase::GetRegister( uint16_t addr, uint8_t *value )
{
	return ReadRegister(addr, value);
}

int CameraBase::SetRegisters( uint16_t addr, uint8_t *value, int n )
{
	int res = MS_SUCCESS;
	for (int i=0; i<n; i++)
	{
		res = SetRegister(addr+i, value[i]);
		if (res != MS_SUCCESS)
		{
			return res;
		}
	}
	return res;
}

int CameraBase::GetRegisters( uint16_t addr, uint8_t *value, int n )
{
	int res = MS_SUCCESS;
	for (int i=0; i<n; i++)
	{
		res = GetRegister(addr+i, &(value[i]));
		if (res != MS_SUCCESS)
		{
			return res;
		}
	}
	return res;
}

int CameraBase::WaitForAvailable(int ms/*=3000*/)
{
	uint8_t status;
	int ret = ReadRegister(SETTINGS_WORK_STATUS, &status);
	if (ret != MS_SUCCESS)
	{
		return ret;
	}

	bool busy = (status & BUSY_STATE) ? true : false;
	while (busy)
	{
		SLEEP(1);
		ms--;
		if (ms <= 0)
		{
			return MS_TIMEOUT;
		}

		ret = ReadRegister(SETTINGS_WORK_STATUS, &status);
		if (ret != MS_SUCCESS)
		{
			return ret;
		}

		busy = (status & BUSY_STATE) ? true : false;
	}

	return MS_SUCCESS;
}






}